/**
 * @file JPSPlanner.h
 * @brief JPS (Jump Point Search) 算法路径规划器 - 完善版本
 * @details
 * JPS算法核心思想：
 * 1. 识别跳点（Jump Points）：有强制邻居的节点或目标节点
 * 2. 在直线和对角线方向上进行跳跃，跳过对称路径
 * 3. 只扩展必要的节点，大幅提高搜索效率
 *
 * 关键概念：
 * - 自然邻居：从父节点方向继续移动的方向
 * - 强制邻居：由于障碍物而必须考虑的新方向
 * - 跳点：有强制邻居的节点或目标节点
 */

#ifndef JPS_PLANNER_H
#define JPS_PLANNER_H

#include "core/PlannerBase.h"
#include <queue>
#include <unordered_map>
#include <vector>

namespace PathPlanning
{
/**
 * @brief JPS算法配置参数
 */
struct JPSConfig : public PlannerConfig
{
    bool        enableDiagonal    = true; // 是否允许对角线移动
    double      obstacleThreshold = 128.0;
    std::string heuristicType     = "euclidean";
    double      weight            = 1.0;
    int         maxJumpDistance   = 100; // 最大跳跃距离

    JPSConfig()
    {
        plannerName = "JPS";
    }
};

/**
 * @brief JPS搜索节点
 */
struct JPSNode : public Node
{
    GridCell gridCell;

    JPSNode(const GridCell& cell, const Point& worldPos) : gridCell(cell)
    {
        position = worldPos;
    }
};

/**
 * @brief 方向定义
 */
struct Direction
{
    int dx, dy;

    Direction(int dx = 0, int dy = 0) : dx(dx), dy(dy) {}

    bool isDiagonal() const
    {
        return dx != 0 && dy != 0;
    }
    bool isCardinal() const
    {
        return !isDiagonal();
    }

    bool operator==(const Direction& other) const
    {
        return dx == other.dx && dy == other.dy;
    }

    // 获取垂直方向（用于强制邻居检测）
    Direction getPerpendicular() const
    {
        if (isDiagonal())
        {
            return Direction(dy, dx); // 交换dx, dy
        }
        else
        {
            return Direction(-dy, -dx); // 旋转90度
        }
    }
};

/**
 * @brief JPS节点比较器
 */
struct JPSNodeCompare
{
    bool operator()(const std::shared_ptr<JPSNode>& a, const std::shared_ptr<JPSNode>& b) const
    {
        return a->fCost > b->fCost;
    }
};

class JPSPlanner : public PlannerBase
{
public:
    JPSPlanner(std::shared_ptr<Map> map);

    PlanningResult plan(const Point& start, const Point& goal) override;

private:
    // 预定义方向
    std::vector<Direction> allDirections;
    std::vector<Direction> cardinalDirections;
    std::vector<Direction> diagonalDirections;

    /**
     * @brief 初始化方向向量
     */
    void initializeDirections();

    /**
     * @brief 计算启发式代价
     */
    double calculateHeuristic(const Point& from, const Point& to);

    /**
     * @brief 获取节点的自然邻居方向
     */
    std::vector<Direction> getNaturalDirections(const JPSNode* node);

    /**
     * @brief 在给定方向上进行跳跃
     */
    std::shared_ptr<JPSNode> jump(const GridCell& current, const Direction& dir, const GridCell& goal);

    /**
     * @brief 直线方向跳跃
     */
    std::shared_ptr<JPSNode> jumpStraight(const GridCell& current, const Direction& dir, const GridCell& goal);

    /**
     * @brief 对角线方向跳跃
     */
    std::shared_ptr<JPSNode> jumpDiagonal(const GridCell& current, const Direction& dir, const GridCell& goal);

    /**
     * @brief 检查节点是否有强制邻居
     */
    bool hasForcedNeighbor(const GridCell& cell, const Direction& dir);

    /**
     * @brief 检查基本方向的强制邻居
     */
    bool hasForcedNeighborCardinal(const GridCell& cell, const Direction& dir);

    /**
     * @brief 检查对角线方向的强制邻居
     */
    bool hasForcedNeighborDiagonal(const GridCell& cell, const Direction& dir);

    /**
     * @brief 重建路径
     */
    std::vector<Pose>
    reconstructPath(const std::unordered_map<GridCell, std::shared_ptr<JPSNode>, GridCellHash>& allNodes,
                    const std::shared_ptr<JPSNode>&                                             endNode);

    /**
     * @brief 调试函数：打印搜索状态
     */
    void printSearchStatus(int iteration, const GridCell& current, int directions, int jumpPoints, size_t openSize,
                           size_t closedSize);
};

} // namespace PathPlanning

#endif